#!/bin/env python3
# -*- coding: utf-8 -*-

'''基于光学图像的机械臂状态定位

在机械臂臂节上布置特征点（标记物），然后使用若干个光学相机对其实施成像，
再根据像的位置，估计机械臂的转角状态值。
'''
import pickle
import math as m
import numpy as np
import nllms

def transform_dhparam(dhparam):
    "由DH参数推出，坐标变换矩阵，DH参数顺序：(theta, d, a, alpha)"
    theta, d, a, alpha = dhparam
    cth, sth = m.cos(theta), m.sin(theta)
    cal, sal = m.cos(alpha), m.sin(alpha)
    return np.array(
       (
        (cth  ,  -sth*cal ,  sth*sal,  cth*a ),
        (sth  , cth*cal   , -cth*sal,  sth*a ),
        ( 0   ,   sal     ,     cal ,   d   ),
        ( 0   ,     0     ,     0   ,   1   ),
       ), dtype = float
    )

def der_transform_dhparam(dhparam):
    "由DH参数推出，坐标变换矩阵的偏导数，DH参数顺序：(theta, d, a, alpha)"
    theta, _d, a, alpha = dhparam
    cth, sth = m.cos(theta), m.sin(theta)
    cal, sal = m.cos(alpha), m.sin(alpha)
    return np.array(
       (
        (-sth  , -cth*cal  ,  cth*sal, -sth*a ),
        ( cth  ,  -sth*cal ,  sth*sal,  cth*a ),
        (     0   ,   0   ,  0 ,  0  ),
        (     0   ,   0   ,  0 ,  0  ),
       ), dtype = float
    )

def deg2rad(it):
    "把一个列表的数值，从 *度* 转换成 *弧度*"
    return np.array(it) * (m.pi/180.0)

def rad2deg(it):
    "把一个列表的数值，从 *弧度* 转换成 *度*"
    return np.array(it) * (180.0/m.pi)

class ArmSystem:
    """机械臂定位求解系统

    机械臂系统有一个多关节的机械臂、布置于臂节上的特征标记点及一个成像系统组成，
    使用关节转角表示机械臂的运动状态（自由度），成像系统由多个相机组成，
    通过对机械臂上特征点进行光学成像，并提取各特征点的图像位置，逆求解，
    得到机械臂的状态——关节转角
    """

    def __init__(self, dhParams, rkjs, cameraRT):
        """使用DH参数、相机参数构造
        
        dhParas  机械臂DH参数，N*(theta, d, a, alpha), theta + initVal as initial value for iterate
        rkjs     标记点在局部坐标系中的位置, M*((x,y,z), k)，共有M个点，k为臂节坐标系编号，0起始
        cameraRt 观测相机的坐标变换矩阵参数, C*(R, t)，相机坐标系到总体坐标系的转换"""
        self.dhParams = dhParams
        self.nVar = len(dhParams)
        self.rkjs = rkjs
        self.cameraRT = cameraRT

    def solve_forward(self, thetas):
        """正向求解，输入参数为：
        thetas  :  机械臂工作空间内的位置，关节转角的列表"""
        dhparams_mod = [
            (theta+theta_off, d, a, alpha)
            for theta, (theta_off, d, a, alpha) in zip(thetas, self.dhParams)
        ]
        Ris = [transform_dhparam(dh) for dh in dhparams_mod]
        posi0s = []  # 总体坐标系下的坐标值，第一维度为点序
        for (x, y, z), index in self.rkjs:
            # RLi = np.diag(np.array((1, 1, 1, 1), dtype=float))
            posi4i = np.array((x, y, z, 1.0), dtype=float)  # 构造齐次坐标式
            # 从局部坐标系开始，依次左乘上一级的坐标变换矩阵，得总体坐标系下的值
            for i in reversed(range(index+1)):  # 包含编号所在的坐标系矩阵
                posi4i = np.dot(Ris[i], posi4i)
            posi0s.append(posi4i[:3])  # only first 3 components
            # orie0 = m.atan2(RLi[2,1], RLi[2,2]), m.asin(RLi[2,0]), m.atan2(RLi[1,0], RLi[0,0])
            # print("transform %s from local to global at: %s, %s" % ((x,y,z), posi40, rad2deg(orie0),))
        pass
        return posi0s

    def solve_forward_velo(self, thetas, var_ith):
        """正向求解，输入参数为：
        thetas  :  机械臂工作空间内的位置，关节转角的列表
        var_ith :  产生运动的关节编号"""
        dhparams_mod = [ (theta+theta_off, d, a, alpha)
            for theta, (theta_off, d, a, alpha) in zip(thetas, self.dhParams)]
        Ris = [transform_dhparam(dh) for dh in dhparams_mod]
        velos = []
        for (x, y, z), index in self.rkjs:
            if var_ith > index:  #产生运动的关节在此点臂节之后，则此臂节上速度为零
                velo = np.array((0.0,0,0,0), dtype=float)
            else:
                velo = np.array((x, y, z, 1.0), dtype=float)  # 构造齐次坐标式
                for i in reversed(range(index+1)):  # 包含编号所在的坐标系矩阵
                    velo = np.dot(Ris[i], velo)
                    if i == var_ith:  # 在求导的关节上，后左乘一个求导变换矩阵
                        velo = np.dot(np.array((
                            (0,-1,0,0),
                            (1,0,0,0),
                            (0,0,0,0),
                            (0,0,0,0),
                        ), dtype=float), velo)
            velos.extend(velo)
        return velos

    def cal_jacobi_f(self, thetas):
        """臂节上的特征点在总体坐标系中位置矢量对关节转角的雅比矩阵。输入参数为：
        thetas  :  机械臂工作空间内的位置，关节转角的列表"""
        dhparams_mod = [ (theta+theta_off, d, a, alpha)
            for theta, (theta_off, d, a, alpha) in zip(thetas, self.dhParams)]
        Ris = [transform_dhparam(dh) for dh in dhparams_mod]
        dRis = [der_transform_dhparam(dh) for dh in dhparams_mod]
        jacobis = []
        # print("lens %s, %s" % (len(thetas), len(R
        # 
        # 
        # 
        # 
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        
        ...............................................................................................................................................................................................is)))
        for i in range(len(thetas)):
            dposiL = []
            for (x, y, z), index in self.rkjs:
                if i > index:  # out of local coord
                    dposiL.extend((0.,0.,0.))
                    continue
                dposi4i = np.array((x, y, z, 1.0), dtype=float)  # 构造齐次坐标式
                for i2 in reversed(range(index+1)):  # 包含编号所在的坐标系矩阵
                    if i2 == i:  # 遍历时，坐标系序号同于求导坐标系，使用求导矩阵
                        dposi4i = np.dot(dRis[i2], dposi4i)
                    else:
                        dposi4i = np.dot(Ris[i2], dposi4i)
                dposiL.extend(dposi4i[:3])
            jacobis.append(dposiL)
        posiL = []
        for (x, y, z), index in self.rkjs:
            posi4i = np.array((x, y, z, 1.0), dtype=float)  # 构造齐次坐标式
            for i2 in reversed(range(index+1)):  # 0, 1, ... index
                posi4i = np.dot(Ris[i2], posi4i)
            posiL.extend(posi4i[:3])
        pass
        # 返回值：雅可比矩阵（实际为转置结果，行为同一自变量）、位置点（合并为一行）
        return jacobis, posiL

    def projection_forward(self, idsPointCamera, thetas):
        '''从机械臂转角状态计算得特征点在相机中的成像位置
        thetas   各臂节转角状态值，弧度值
        idsPointCamera  形如 N*(point, cam)组合列表，指定哪点在哪相机中，
        再从系统配置中提取相机安装参数
        返回值：形如 N*(x_bar, y_bar) 的投影位置点
        '''
        posi0s = self.solve_forward(thetas)
        xys = []
        for pointId, camId in idsPointCamera:
            r0 = posi0s[pointId]  # 前面返回二维列表
            Rc, tc = self.cameraRT[camId]
            # 变换到相机坐标系内， $ \vec r_c = \vec R_c^T ( \vec r_0 - \vec t_c) $
            xc, yc, zc = np.dot(np.transpose(Rc), np.subtract(r0, tc))
            # 在相机坐标系内完成投影，透视法
            hat_xy = xc/zc, yc/zc
            xys.append(hat_xy)
        return xys

    def cal_xy_jacobi_f(self, idsPointCamera, thetas):
        """从投影逆到关节转角的雅可比矩阵，输入参数：
        idsPointCamera  臂节上特征点在不同相机中的投影位置，形如 N*(pointId, camId)
        thetas          关节转角值
        返回值：  相机内投影坐标的雅可比矩阵（实际为转置）及投影点位置"""
        # 先计算得，总体坐标系下的位矢及其雅可比矩阵，行为自变量，列为点序展开 3*j
        jacT, posiL = self.cal_jacobi_f(thetas)
        jac_xys, hat_xys = [], []
        for pointId, camId in idsPointCamera:
            r0 = posiL[3*pointId:3*pointId+3]  # 此点在总体坐标系下的表示
            Rc, tc = self.cameraRT[camId]  # 提取出与相机序号相应的变换矩阵及偏移量
            # 变换到相机坐标系内， $ \vec r_c = \vec R_c^T ( \vec r_0 - \vec t_c) $
            xc, yc, zc = np.dot(np.transpose(Rc), np.subtract(r0, tc))
            # 在相机坐标系内完成投影，透视法
            hat_xy = xc/zc, yc/zc
            hat_xys.extend(hat_xy)
            jac_j = []
            for i in range(len(thetas)):  # 逐个求雅比系数
                d_r0 = jacT[i][3*pointId:3*pointId+3]  # 总体坐标系下位矢的雅可比系数
                jx, jy, jz = np.dot(np.transpose(Rc), d_r0)  # 得到相机坐标系下位矢的雅可比系数
                # $ (\bar x, \bar y) = (xc/zc, yc/zc) $ 的链式求导
                jac_x = jx/zc - xc/(zc*zc)*jz
                jac_y = jy/zc - yc/(zc*zc)*jz
                jac_j.append((jac_x, jac_y))
            jac_xys.append(jac_j)  # jac_xy 维度有点怪：[xy_{j,c}, theta, (x,y)]
        # 交换第一、二维度，并与点内序（X、Y）——即新的第二、三维度合并
        jac_xy_T = np.array(list(zip(*jac_xys))).reshape((-1, len(idsPointCamera)*2))
        return jac_xy_T, hat_xys

### 本文件需要提供两个函数，即 优化目标差异及雅可比矩阵，作为外部C语言库函数的回调函数，
### 此为 C 语言定义的调用规范：
# extern int fCb(const gsl_vector*x, void*paramsCb, gsl_vector*f);  // 目标差异
# extern int jacCb(const gsl_vector*x, void*paramsCb, gsl_matrix*J);  // 雅可比矩阵

## 下面部分为 依据臂节上特征点空间位置估计转角状态时的
# @nllms.ffi.def_extern()
# def fCb(x, armHandle, f):  # 目标差异，估计值与实测量的差
#     # x, f 为向量 
#     arm = nllms.ffi.from_handle(armHandle)  # 转换到 Arm 对象
#     vars = [nllms.lib.gsl_vector_get(x, i) for i in range(x.size)]
#     posi = arm.solve_forward(vars)
#     delta_f = []
#     for (x, y, z), (x0, y0, z0) in zip(posi, arm.r0js):
#         delta_f.extend((x-x0, y-y0, z-z0),)
#     for i, v in enumerate(delta_f):
#         nllms.lib.gsl_vector_set(f, i, v)
#     return 0

# @nllms.ffi.def_extern()
# def jacCb(x, armHandle, jac):
#     arm = nllms.ffi.from_handle(armHandle)
#     vars = [nllms.lib.gsl_vector_get(x, i) for i in range(x.size)]
#     jacT, _posi = arm.cal_jacobi_f(vars)
#     for ivar, jacCol in enumerate(jacT):
#         for jobj, v in enumerate(jacCol):
#             nllms.lib.gsl_matrix_set(jac, jobj, ivar, v)
#     return 0

## 下面部分为 依据臂节上特征点的像投影位置估计转角状态时的
@nllms.ffi.def_extern()
def fCb(x, armHandle, f):  # 投影法之目标差异，估计值与实测之差
    arm = nllms.ffi.from_handle(armHandle)
    thetas = [nllms.lib.gsl_vector_get(x, i) for i in range(x.size)]
    xys, idsPC = zip(*arm.xys)  # N*[(x, y), (pointId, camId)]
    hat_xys = arm.projection_forward(idsPC, thetas)
    for i, ((hat_x, hat_y), (x, y)) in enumerate(zip(hat_xys, xys)):
        nllms.lib.gsl_vector_set(f, 2*i, hat_x - x)
        nllms.lib.gsl_vector_set(f, 2*i+1, hat_y - y)
    return 0

@nllms.ffi.def_extern()
def jacCb(x, armHandle, jac):
    arm = nllms.ffi.from_handle(armHandle)
    thetas = [nllms.lib.gsl_vector_get(x, i) for i in range(x.size)]
    _xys, idsPC = zip(*arm.xys)  # N*[(x, y), (pointId, camId)]
    jacT, _hat_xys = arm.cal_xy_jacobi_f(idsPC, thetas)
    for ivar, jacCol in enumerate(jacT):
        for jobj, v in enumerate(jacCol):
            nllms.lib.gsl_matrix_set(jac, jobj, ivar, v)
    return 0

def transformByRPY(rpy):
    "由RPY计算变换矩阵，(Roll,Pitch,Yaw)"
    phi, theta, psi = rpy
    cphi, sphi = m.cos(phi), m.sin(phi)
    rotX = (
        (1.0,  0,   0,),
        (0., cphi, -sphi),
        (0., sphi, cphi),
    )
    cth, sth = m.cos(theta), m.sin(theta)
    rotY = (
        (cth,  0., sth),
        (0.,  1.0, 0. ),
        (-sth, 0., cth)
    )
    cps, sps = m.cos(psi), m.sin(psi)
    rotZ = (
        (cps, -sps, 0.),
        (sps,  cps, 0.),
        (0.,  0.,  1.0),
    )
    # 从随体坐标系到总体坐标系的变换矩阵，为依次旋转，先绕X轴，再Y轴，再Z轴，依次左乘
    return np.dot(rotZ, np.dot(rotY, rotX))

def cameraTransformByRPY(rpy):
    '''相机的变换矩阵，增加一个额外的变换
    正常的坐标系中，X向前，Z向上，右手坐标系，而在相机中，Z轴从光心指向物体，X向右，Y朝下，
    如此，在XY平面上，更类似于计算机专业对图像坐标系的约定。
    '''
    extraRot = (
        (0.,  0., 1,),
        (-1., 0,  0.),
        (0, -1.0, 0.)
    )
    R = transformByRPY(rpy)
    return np.dot(R, extraRot)

def cameraProjection3D(xyzs, viewAngle=63.0):
    "将相机坐标系内的三维空间点投影到相机的相平面上，指定视角大小，图像分辨率取用1080P"
    w, h = 1920, 1080
    ox, oy = w/2, h/2
    #m.sqrt(w*w+h*h)
    dpAngle = w/(2.0*m.tan(viewAngle*m.pi/360))
    bar_xys = [(dpAngle*x/z+ox, dpAngle*y/z+oy) for x,y,z in xyzs]
    return bar_xys

def cameraProjectionHomo(xys, viewAngle=63.0):
    "将相机坐标系内的齐次坐标点投影到相机的相平面上，指定视角大小，图像分辨率取用1080P，齐次坐标只保留2项"
    w, h = 1920, 1080
    ox, oy = w/2, h/2
    # m.sqrt(w*w+h*h)
    pixel_per_unit = w/(2.0*m.tan(0.5*viewAngle*m.pi/180))
    pixel_xys = [(pixel_per_unit*x+ox, pixel_per_unit*y+oy) for x,y in xys]
    return pixel_xys

def cameraPixelToHomo(xys, viewAngle=63.0):
    "从像素坐标系转换到齐次式的坐标"
    w, h = 1920, 1080
    ox, oy = w/2, h/2
    unit_per_pixel = (2.0*m.tan(0.5*viewAngle*m.pi/180))/w # m.sqrt(w*w+h*h)  # 每像素对应的齐次变化量
    bar_xys = [((x-ox)*unit_per_pixel, (y-oy)*unit_per_pixel) for x,y in xys]
    return bar_xys

def createArm():
    dhparamsD = (  # 为方便计，采用度数表示两转角
        # theta, d, a, alpha
        (90,  .000, .008,  90),
        (90,  .232, .034,  90),
        (90,  .000, .385,  0.),
        (-90, .000, .082, -90),
        (-90, .367, .000,  0),
    )
    thetasD, ds, ass, alphasD = zip(*dhparamsD)
    thetas = deg2rad(thetasD)
    alphas = deg2rad(alphasD)
    dhparams = list(zip(thetas, ds, ass, alphas))  # 至此，转换到弧度表示
    localPositions = [
        ((-.3, -.1, 0.),2,),
        ((0., 0., -.08),2,),
        ((.1, 0, 0.)   ,3,),
        ((0., -.1, .3) ,3,),
    ]
    camRt = [(
        cameraTransformByRPY(deg2rad((-45.,65,-35))), (0., 0.5, 1.20 ),  #// t, 左上方1米高处，向右偏斜
    ), (
        cameraTransformByRPY(deg2rad((0.,65,0))), (0., 0., 1.20),  #// t, 正上方1米高处，在总坐标系中，X为正前，Z为正上，Y为左方向
    )]
    arm = ArmSystem(dhparams, localPositions, camRt)
    return arm

def testTransrom():
    tf = cameraTransformByRPY(deg2rad((0.,45,0)))
    tt = (0., 0., 1.0)
    x_gs = ((1.0, 0., 0.), (1.2, 0.,0.))
    x_ls = ((0.,0.,1.41421,), (0., -0.2*0.7071, 2.2*0.7071))
    print('trans test1 ', x_gs, np.add(np.transpose(np.dot(tf, np.transpose(x_ls))), tt))
    tf = cameraTransformByRPY(deg2rad((0.,45,-30.)))
    tt = (0., 0., 1.0)
    x_gs = ((1.0, 0., 0.),)
    x_ls = ((-.5,(1.0-0.5*m.sqrt(3.))/m.sqrt(2.),(1.0+0.5*m.sqrt(3.))/m.sqrt(2.),), )
    print('trans test2 ', x_gs, np.add(np.transpose(np.dot(tf, np.transpose(x_ls))), tt))

def test1():
    "测试简单的正向求解到总体坐标系位置"
    arm = createArm()
    thetas = deg2rad((30, 45, -10, 100, 20, -35))
    posi0 = arm.solve_forward(thetas)
    print("result: position in global coordinate:", posi0)

def test1A():
    "测试正向求解"
    arm = createArm()
    arm.rkjs = [
        ((0., 0., 0.), 4)
    ]
    # thetas = deg2rad((30, 45, -10, 100, 20, -35))
    theta_file, xyz_file = 'inv-theta-n5-12346.pkl', 'xyz-end5.pkl' #'theta0.pkl', 'xyz-end0.pkl'
    thetas = pickle.load(open(theta_file, 'rb'))
    posis = [arm.solve_forward(theta) for theta in thetas]
    pickle.dump(posis, open(xyz_file, 'wb'))

def test2():
    "测试雅可比矩阵计算"
    arm = createArm()
    thetas = deg2rad((30, 45, -10, 100, 20, -35))
    cols = len(arm.rkjs)*3
    posi0 = arm.solve_forward(thetas)
    velos = arm.solve_forward_velo(thetas,  0)
    print("velos :", velos)
    posi01d = np.array(np.array(posi0).flat)
    jac, _posi = arm.cal_jacobi_f(thetas)
    print("test", _posi, posi0)
    print("jacobi : ", jac)
    jac = np.array(jac)
    print("size of jac: ", jac.shape)
    JTJ = np.dot(jac, np.transpose(jac))
    print("JTJ is : ", JTJ)
    print("the det of JTJ is: ", np.linalg.det(JTJ))
    print("position: ", ('%9.5f '*cols)% tuple(posi01d) )
    detla_theta = 0.0001
    for i in range(len(thetas)):
        thetas[i] += detla_theta
        posi1 = arm.solve_forward(thetas)
        thetas[i] -= detla_theta
        posi11d = np.array(np.array(posi1).flat)
        print("posi 1: ", ('%9.5f '*cols) % tuple( posi11d))
        print("df_dth: ", ('%9.3f '*cols) % tuple((posi11d-posi01d)*(1.0/detla_theta)))
        print("jacobi: ", ('%9.3f '*cols) % tuple(jac[i]))
        print()

def test3():
    "测试逆向求解，从总体坐标系的位置求解"
    arm = createArm()
    # thetas = deg2rad((30, 45, -10, 100, 20, -35))
    thetas = deg2rad((10, 40, -0, 60, 2, -5))  # initial value
    # globalPositions = [
        # [0.30643085, 0.06593387, 0.10411502], 
        # [0.58921402, 0.20175336, 0.01474639], 
        # [0.71854637, 0.34174315, 0.07131494],  
        # [0.49052247,  0.53669212, -0.07010642],
    # ]
    globalPositions = [  # 增加一些误差
        # (.307, .066, .105),
        # (.590, .202, .015),
        # (.720, .340, .070),
        # (.490, .540, -.07),
        [0.30643085, 0.06593387, 0.10411502], 
        [0.58921402, 0.20175336, 0.01474639], 
        [0.71854637, 0.34174315, 0.07131494],  
        [0.49052247,  0.53669212, -0.07010642],
    ]
    arm.r0js = globalPositions
    varsC = nllms.ffi.new("double []", arm.nVar)
    for i, v in enumerate(thetas[:5]):  # 初始化
        varsC[i] = v
    paramsCb = nllms.ffi.new_handle(arm)
    ret = nllms.lib.search_by_nlinear_lms(len(globalPositions)*3, arm.nVar, paramsCb, 1.e-4, varsC)
    print("search return {ret}".format(ret=ret,))
    vs = [ varsC[i] for i in range(arm.nVar)]
    print("\n\nresult: ", rad2deg(vs))

def test4():
    "测试投影正向求解"
    arm = createArm()  # 臂上有四个点，并有两个相机
    thetas = deg2rad((30, 45, -10, 100, 20))
    idsPointCam = [(p, c) for c in range(2) for p in range(4)]
    xys = arm.projection_forward(idsPointCam, thetas)
    xys = cameraProjectionHomo(xys, 63.0)
    # print(xys)
    print('point in every camera:')
    for xy in xys:
        print("(%+.1f, %+.1f)," % xy)
    pass

def test4stdio():
    "以文件IO的投影正向求解，解到图像坐标系，以像素值表示。"
    arm = createArm()  # 臂上有四个点，并有两个相机
    # thetas = deg2rad((30, 45, -10, 100, 20))
    idsPointCam = [(p, c) for c in range(2) for p in range(4)]
    res = []
    for line in sys.stdin:
        thetas = [float(v) for v in line.split()]  # 切分，转类型，已是弧度值
        xys = arm.projection_forward(idsPointCam, thetas)
        xys = cameraProjectionHomo(xys, 63.0)
        res.append(xys)
        for xy in xys:
            print("%.1f %.1f " % xy, end='')
        print()
    pickle.dump(res, open('xys1.pkl', 'wb'))
    pass

def test5():
    "测试投影逆向求解"
    arm = createArm()
    thetas = deg2rad((10, 40, -0, 60, 2, -5))  # initial value
    xys = [ # pixel ,u,v
        ((+1116.2, 714.6),(0, 0)),
        ((+958.3, +388.1),(1, 0)),
        # ((+838.4, +183.8),(2, 0)),
        ((+535.2, +438.9),(3, 0)),
    ]
# [((1016.852321103877, 978.15951606343697), (1, 0)), ((798.88206270646242, 1123.7945206467136), (2, 0)), ((390.45472435468093, 1274.3220309704066), (3, 0))]
    thetas = (1.5,  0.0000, -1., 2.,  0.0000)
    xys = [((1030.3833462626701, 947.70954725888078), (1, 0)),
     ((809.34185917680281, 1057.6646229806238), (2, 0)),
      ((394.5078205275197, 1199.5084893535955), (3, 0))]
    # 每次的逆向求解，先把相机对特征点的观测情况存入机械臂系统，成为临时状态
    xys1, ids1 = zip(*xys)
    xys1 = cameraPixelToHomo(xys1, 63.0)
    arm.xys = list(zip(xys1, ids1))
    varsC = nllms.ffi.new("double []", arm.nVar)
    for i, v in enumerate(thetas[:arm.nVar]):
        varsC[i] = v
    paramCb = nllms.ffi.new_handle(arm)
    ret = nllms.lib.search_by_nlinear_lms(len(xys)*2, arm.nVar, paramCb, 1.e-5, varsC)
    print("search return {ret}".format(ret=ret,))
    vs = [ varsC[i] for i in range(arm.nVar)]
    print("\n\nresult: ", rad2deg(vs))

def test5file(pklFile='xys1.pkl', outFile='theta.pkl'):
    "测试投影逆向求解"
    points_group = (
        (0, 0), (1, 0), (2, 0), (3, 0),
        (0, 1), (1, 1), (2, 1), (3, 1),
    )
    selected_points = (5,4,7)
    arm = createArm()
    # thetas = deg2rad((10, 40, -0, 60, 2, -5))  # initial value
    thetas = (1.5,  0.0000, -1., 2.,  0.0000)
    varsC = nllms.ffi.new("double []", arm.nVar)
    for i, v in enumerate(thetas[:arm.nVar]):
        varsC[i] = v
    paramCb = nllms.ffi.new_handle(arm)
    point_projection = pickle.load(open(pklFile, 'rb'))
    thetas = []
    for point_line in point_projection:
        xys = [(point_line[ind], points_group[ind]) for ind in selected_points]
        # print("to solve for ", xys)
        xys1, ids1 = zip(*xys)
        xys1 = cameraPixelToHomo(xys1, 63.0)  # 从像素坐标转换到齐次坐标
        arm.xys = list(zip(xys1, ids1))
        ret = nllms.lib.search_by_nlinear_lms(len(xys)*2, arm.nVar, paramCb, 1.e-5, varsC)
        if ret != 0:
            print(f"gls nlinear search return {ret}, fails")
            break
        #print("search return {ret}".format(ret=ret,))
        vs = [ varsC[i] for i in range(arm.nVar)]
        # print("%7.4f %7.4f %7.4f %7.4f %7.4f\n", vs)
        thetas.append(vs)
    pickle.dump(thetas, open(outFile, 'wb'))

def main():
    pass

if __name__=="__main__":
    import sys
    # test4stdio()
    test4()
    #test1A()
    # test5file('noise-xys1.pkl', 'inv-theta-n3-457.pkl')
    # testTransrom()
